/*
 * @Description: 
 * @Author: Sang Hao
 * @Date: 2021-09-15 17:12:01
 * @LastEditTime: 2021-10-27 14:23:45
 * @LastEditors: Sang Hao
 */
#include "lidar_slam/subscriber/odometry_subscriber.hpp"

namespace lidar_slam {
/**
 * @description: 构造函数
 * @param  {*}
 * @return {*}
 * @param {string} topic_name
 * @param {size_t} buff_size
 */	
OdometrySubscriber::OdometrySubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size)
	: nh_(nh) {
	subscriber_ = nh.subscribe(topic_name, buff_size, &OdometrySubscriber::msg_callback, this);
}

/**
 * @description: 收到里程计消息后，再将其转回eigen的pose_data数据 
 * @param  {*}
 * @return {*}
 */
void OdometrySubscriber::msg_callback(const nav_msgs::OdometryConstPtr& odom_msg_ptr) {
	PoseData pose_data;
	pose_data.time = odom_msg_ptr->header.stamp.toSec();

	pose_data.pose(0, 3) = odom_msg_ptr->pose.pose.position.x;
	pose_data.pose(1, 3) = odom_msg_ptr->pose.pose.position.y;
	pose_data.pose(2, 3) = odom_msg_ptr->pose.pose.position.z;

	Eigen::Quaternionf q;
	q.x() = odom_msg_ptr->pose.pose.orientation.x;
	q.y() = odom_msg_ptr->pose.pose.orientation.y;
	q.z() = odom_msg_ptr->pose.pose.orientation.z;
	q.w() = odom_msg_ptr->pose.pose.orientation.w;

	pose_data.pose.block<3, 3>(0, 0)  = q.matrix();

	new_pose_data_.push_back(pose_data);
}

/**
 * @description: 解析数据，插入到deque后
 * @param  {*}
 * @return {*}
 */
void OdometrySubscriber::ParseData(std::deque<PoseData>& deque_pose_data) {
	if (new_pose_data_.size() > 0) {
		deque_pose_data.insert(deque_pose_data.end(), new_pose_data_.begin(), new_pose_data_.end());
		new_pose_data_.clear();
	}
}
}

